

/*
 
 
     // Test existing fronts against the expansion prisms
     
     // In doing this test, 
     /*          
     
     v
     p'  /\   n'
     \ /  \ /
     p    n
     
     - obviously ignore prism edges (p, v), and (v, n)
     
     - ignore spherical regions around p and n (radius = small fraction of edge lengths)
     to prevent spurious (p, p') or (n, n') collisions with prisms near p, n due to numerical innaccuracy.  (Though cannot ignore (p, p'), (n, n') entirely, since these edges may in fact intersect the prisms).
     

 */

/*
 
     On finding a closest hit c within the prisms,  simply construct
     an epsilon bridge between (v, c) by sliding parallel to (v, c)
     [creating two epsilon edges or 'struts', which can be connected to form the bridge].
     
     This allows the front expansion to proceed safely topologically.
     
     Then this thin bridge can be removed via edge collapse later
     (as soon as the 4 bridge 'strut' vertices are not on any front).
     [Can keep track of the existing bridges, and cull them periodically, or just do
     as a post-process if preferred].
 
 */


/*
     Collision detection can be done efficiently by
     associating every contiguous 64 vertex front section with a BVH,
     and keeping a BVH of these BVHs. Then we only have to rebuild a 64 vertex
     BVH per step.  BVHs which do not contain any vertices which lie on the current set of fronts must be periodically culled [once very 64 vertices added, say].
     Split/merges will require computation of which subset of BVHs must be rebuilt.
     
     Possibly keep a map from vertex to BVH (each vertex is contained in 
     only one BVH). Then it is easy to determine which BVHs are affected by
     a split/merge, according to which vertices are involved.
 
 
 */





#ifndef POLYGONIZER4_H
#define POLYGONIZER4_H

#include <list>
#include <vector>
#include <queue>
#include "Vec4.h"
#include "Matrix3.h"
#include "Aabb.h"
#include "float.h"
#include "Geometry.h"
#include <cmath>

#include "AabbTree.h"

// FOR DEBUGGING PURPOSES
#include <OpenGL/OpenGL.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>

#include <stdio.h>


/* note #undef's at end of file */
#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)



float ran2(long *idum)
{
	int j;
	long k;
	static long idum2=123456789;
	static long iy=0;
	static long iv[NTAB];
	float temp;
	
	if (*idum <= 0) {
		if (-(*idum) < 1) *idum=1;
		else *idum = -(*idum);
		idum2=(*idum);
		for (j=NTAB+7;j>=0;j--) {
			k=(*idum)/IQ1;
			*idum=IA1*(*idum-k*IQ1)-k*IR1;
			if (*idum < 0) *idum += IM1;
			if (j < NTAB) iv[j] = *idum;
		}
		iy=iv[0];
	}
	k=(*idum)/IQ1;
	*idum=IA1*(*idum-k*IQ1)-k*IR1;
	if (*idum < 0) *idum += IM1;
	k=idum2/IQ2;
	idum2=IA2*(idum2-k*IQ2)-k*IR2;
	if (idum2 < 0) idum2 += IM2;
	j=iy/NDIV;
	iy=iv[j]-idum2;
	iv[j] = *idum;
	if (iy < 1) iy += IMM1;
	if ((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
}
#undef IM1
#undef IM2
#undef AM
#undef IMM1
#undef IA1
#undef IA2
#undef IQ1
#undef IQ2
#undef IR1
#undef IR2
#undef NTAB
#undef NDIV
#undef EPS
#undef RNMX



///////////////////////////////////////////////////////////////////////////////////////////



class Polygonizer 
{
public:
	
    
   	Polygonizer(long seed=-1) : m_seed(seed)
	{
		ran2(&m_seed);	
	}
	
	// Pass the functor which defines the level set as the zero value isosurface
    template<typename Functor>
    void Polygonizer::polygonize(const Box3f& bbox, float tol, float scale)
    {
        
        // Look for a good start point
        V3f extents = bbox.max - bbox.min;
        m_extents = extents;
        const int startPointSearchIters = 64*64*64;
        float minValue = FLT_MAX;
        
        V3f startPoint;
        for (int n=0; n<startPointSearchIters; ++n)
        {
            V3f testPoint = bbox.min + V3f(ran2(&m_seed)*extents[0], 
                                           ran2(&m_seed)*extents[1], 
                                           ran2(&m_seed)*extents[2]);
            float testVal = fabs(Functor::value(testPoint));
            if ( testVal<minValue ) 
            {
                startPoint = testPoint;
                minValue = testVal;
            }
        }
        
        // TO DO: (IMPORTANT) a more intelligent distribution of seed points (more than one!), to attempt to capture all 'islands'.
        vector<V3f> seedPoints;
        seedPoints.push_back(startPoint);
        
        m_scale = scale;
        
        // Stamp initial hexagon
        m_tol = tol;
        
        for (int s=0; s<seedPoints.size(); ++s)
        {
            seedHexagon<Functor>(seedPoints[s]);
        }
		
        // DEBUG
        m_currentFront = 0;
        m_currentFrontVertex = m_fronts[0]->m_vertices.begin();
        m_switchFront = false;
    }
	
    
	const Geometry& getGeometry()
	{
		return m_mesh;
	}
	
    
    struct Edge
    {
        // Indices of vertex on edge are, by convention, in order (a, b) = (vertex, following vertex on front).
        Edge() : m_a(-1), m_b(-1) {}
        
        Edge(int a, int b, const Geometry& mesh) : m_a(a), m_b(b)
        {
            m_A = mesh.m_vertices[a];
			m_B = mesh.m_vertices[b];
            m_bbox.extendBy(m_A); 
            m_bbox.extendBy(m_B);
        }
        
        int m_a, m_b;
        V3f m_A, m_B;
        Box3f m_bbox;
    };
    
        
    
    
	struct Front
	{
		struct Vertex
		{
            Vertex(int index) : m_index(index), m_active(true) { }
            
			int m_index;
			bool m_active;
			float m_openingAngle;
            
            // This is the index of the Triangle adjacent to the front which contains the edge which
            // *begins* at this vertex, i.e. is spanned by moving from this vertex to the next vertex.
            int m_triangleIndex;
            
			bool operator<(const Vertex& other) const
			{
				return m_openingAngle < other.m_openingAngle;
			}
		};
		
        int nextVertex(int v)
        {
            list<Vertex>::iterator vertex = getIterator(v);
            list<Front::Vertex>::iterator nextVertex = vertex; nextVertex++;
            if (nextVertex == m_vertices.end()) nextVertex++;
            return nextVertex->m_index;
        }
        
        list<Vertex>::iterator nextIter(list<Vertex>::iterator vertex)
        {
            list<Front::Vertex>::iterator nextVertex = vertex; nextVertex++;
            if (nextVertex == m_vertices.end()) nextVertex++;
            return nextVertex;
        }
        
        int prevVertex(int v)
        {
            list<Vertex>::iterator vertex = getIterator(v);
            list<Front::Vertex>::iterator prevVertex = vertex; prevVertex--;
            if (prevVertex == m_vertices.end()) prevVertex--;
            return prevVertex->m_index;
        }
        
        list<Vertex>::iterator prevIter(list<Vertex>::iterator vertex)
        {
            list<Front::Vertex>::iterator prevVertex = vertex; prevVertex--;
            if (prevVertex == m_vertices.end()) prevVertex--;
            return prevVertex;
        }
        
        list<Vertex>::iterator getIterator(int v)
        {
            return m_vertexIndexToFrontIterator[v];
        }
        
        list<Vertex>::const_iterator getConstIterator(int v)
        {
            return m_vertexIndexToFrontIterator[v];
        }
        
        
        // Insert a vertex with index v into the front list (and mesh) just *after* the specified one p, 
        // i.e. p -> v (p MUST already be a vertex on this front). (If v is the *first* vertex in the front,
        // pass p=NULL).
        void insertVertex(int p, int v)
        {
            list<Front::Vertex>::iterator iter;
            if (p)
            {
                assert(m_vertexIndexToFrontIterator.count(p)>0);
                list<Front::Vertex>::iterator iter = m_vertexIndexToFrontIterator[p];
                iter = m_vertices.insert(iter, Front::Vertex(v));
            }
            else
            {
                m_vertices.push_back(Front::Vertex(v));
                list<Front::Vertex>::iterator iter = m_vertices.end();
                iter--;
            }
        
            m_vertexIndexToFrontIterator[v] = iter;
        }
        
        
        // Circular linked list of vertices in the growing front
		list<Vertex> m_vertices;
        
        
        // This maps the index of a vertex in the geometry to an iterator to its corresponding Vertex in the circular linked list
		map<int, list<Vertex>::iterator> m_vertexIndexToFrontIterator;
        
        
        
        // TO DO: get rid of Vec4 shit in Geometry!!! - just define Geometry in this class
        // (actually, get rid of all inessential shit)
        
        // This maps the index of a vertex on the front to the Triangle which
        // BEGINS at this vertex, i.e. spanned by moving from this vertex to the next vertex.
        // (this could be just stored in Vertex itself... hmm. Yep)
        //map<int, Geometry::Triangle> m_vertexIndexToTriangle;
		
		AabbTree<Edge> m_edgeBVH;
        
        void rebuildEdgeBVH(const Geometry& mesh)
		{
            list<Vertex>::iterator vertex = m_vertices.begin();
            
            vector<Box3f> leafAabbs;
			vector<Edge> leafDatas;
            
            while (vertex != m_vertices.end())
			{
				list<Vertex>::iterator nextVertex = vertex;
				nextVertex++;
				if (nextVertex == m_vertices.end()) nextVertex++;
				
				int vertexA = vertex->m_index;
				int vertexB = nextVertex->m_index;
				Edge edge(vertexA, vertexB, mesh);
				
				const V3f& vA = mesh.m_vertices[vertexA];
				const V3f& vB = mesh.m_vertices[vertexB];
				
				Box3f box;
				box.extendBy(vA);
				box.extendBy(vB);
				leafAabbs.push_back(box);
				leafDatas.push_back(edge);
				
				vertex++;
			}
			
			m_edgeBVH.clear();
			m_edgeBVH.buildTreeTopDown(leafAabbs, leafDatas);
        }
	};
	
	
    typedef list<Front::Vertex>::iterator VertexIter;
    
	/**************************************  data members  **********************************************************/
	
	vector<Front*> m_fronts;
	vector<float> m_vertexRocs;
	Geometry m_mesh;
	
	float m_tol;
	float m_scale;
	long m_seed;
	
    V3d m_extents;
    
	// DEBUG LOGIC FOR TESTS
	bool m_switchFront;
	int m_currentFront;
	list<Front::Vertex>::iterator m_currentFrontVertex;
	
	/************************************************************************************************/
	
    
    void edgeCollapse(int A, int B)
    {

        
    }

	
	static inline void computeTangentSpace(const V3f& N, V3f& T, V3f& B)
	{
		// (N assumed normalized)
		float nx = N[0];
		float ny = N[1];
		float nz = N[2];
		if (nx>0.5f || ny>0.5f) 
			T = V3f(ny, -nx, 0.f);
		else
			T = V3f(-nz, 0.f, nx);
		B = N % T;
		T.normalize();
		B.normalize();
	}
	
    
    ///////////////////////////////////////////////////////////////////////////////////////////
    //  Debug draw stuff
    ///////////////////////////////////////////////////////////////////////////////////////////
    
    
    static inline void drawArrow(const V3f& posA, const V3f& posB, const V3f& color, float scale)
    {
        V3f vec = posB - posA;
        V3f t0, t1, t2;
        computeTangentSpace(vec, t1, t2);
        
        t0 = posB - posA;
        t0.normalize();
        
        V3f lobe1  = posB - t0*scale + t1*scale;
        V3f lobe2  = posB - t0*scale - t1*scale;
        V3f lobe3  = posB - t0*scale + t2*scale;
        V3f lobe4  = posB - t0*scale - t2*scale;
        
        V3f v3ArrowShape[] = {
            V3f(posA), V3f(posB),
            V3f(posB), V3f(lobe1),
            V3f(posB), V3f(lobe2),
            V3f(posB), V3f(lobe3),
            V3f(posB), V3f(lobe4),
        };
        
        glDisable(GL_LIGHTING);
        glLineWidth(1.5f);
        glColor4f(color.x,color.y,color.z,1.0f);
        glEnableClientState(GL_VERTEX_ARRAY);
        glVertexPointer(3, GL_FLOAT, sizeof(V3f), &v3ArrowShape[0].x);
        glDrawArrays(GL_LINES, 0, sizeof(v3ArrowShape)/sizeof(V3f));
        glDisableClientState(GL_VERTEX_ARRAY);
        glColor4f(1.0f,1.0f,1.0f,1.0f);
    }
    
    
    void debug_printFronts()
	{
		printf("\n\n################## fronts ####################\n");
		for (int f=0; f<m_fronts.size(); ++f)
		{
			printf("\n------- front %d --------\n", f);
			Front* front = m_fronts[f];
			list<Front::Vertex>::iterator iter = front->m_vertices.begin();
			while (iter != front->m_vertices.end())
			{
				printf("%d -> ", iter->m_index);
				iter++;
			}
			printf("\n");
            
			printf("Check vertex->iterator map:\n");
			std::map<std::string, std::string>::iterator itermap;
			{
				std::map<int, list<Front::Vertex>::iterator>::iterator itermap;
				for (itermap  = front->m_vertexIndexToFrontIterator.begin(); 
					 itermap != front->m_vertexIndexToFrontIterator.end(); 
					 itermap++) 
				{
					int vertexIndex = itermap->first;
					list<Front::Vertex>::iterator vertex = itermap->second;
					printf("key, val->m_index: %d %d\n", vertexIndex, vertex->m_index);
				}
			}
		}
		
		printf("\n\n");
		for(int vi=0; vi<m_vertexRocs.size(); ++vi)
		{
			printf("Vertex %d roc = %f\n", vi, m_vertexRocs[vi]);
		}
		printf("\n\n");
	}

    void drawFronts() const
	{
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		
		glDisable(GL_TEXTURE_2D);
		glEnable(GL_DEPTH_TEST);
		glDisable(GL_CULL_FACE);
		glCullFace(GL_BACK);
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		
		for (int f=0; f<m_fronts.size(); ++f)
		{
			float fr = float(f)/(m_fronts.size());
			
			const Front* front = m_fronts[f];
			list<Front::Vertex>::const_iterator vertex = front->m_vertices.begin();
			while (vertex != front->m_vertices.end())
			{
				list<Front::Vertex>::const_iterator nextVertex = vertex; 
				nextVertex++;
				if (nextVertex == front->m_vertices.end()) nextVertex++;
				
				const V3f& V = m_mesh.m_vertices[vertex->m_index];
				const V3f& Vnext = m_mesh.m_vertices[nextVertex->m_index];
				float length = (V-Vnext).length();
				
				const V3f& N = m_mesh.m_vertexNormals[vertex->m_index];
				drawArrow(V, V+0.1f*length*N, V3f(0,0,0), length*0.01f);
				V3f dV = 0.1f*(Vnext-V);
				drawArrow(V+dV, Vnext-dV, V3f(1.0-fr,0,fr), length*0.1f);
				
				if (m_currentFrontVertex==vertex)
				{
					DrawUtils::drawCrosshairs(length*0.15f, V, 1.f, 1.f, 0.f, 0.6f);
				}
				if (vertex->m_active)
				{
					DrawUtils::drawCrosshairs(length*0.1f, V, 1.f, 0.f, 0.f, 0.6f);
				}
				else
				{
					DrawUtils::drawCrosshairs(length*0.1f, V, 0.f, 0.f, 1.f, 0.6f);
				}	
                
				vertex++;
			}
			glEnd();
		}
		
		glPopAttrib();
	}
    
    ///////////////////////////////////////////////////////////////////////////////////////////
    
    
	static inline void getDeltas(double& hX, double& hY, double& hZ, const Imath::V3d& wsP, double eps)
    {
		// Numerical recipe to ensure that hX, hY, hZ are 'good' values that minimize round-off error
		// (see http://en.wikipedia.org/wiki/Numerical_differentiation)
        double X = abs(wsP[0]);
        double Y = abs(wsP[1]);
        double Z = abs(wsP[2]);
        hX = eps * std::max(X, 1.0);
        hY = eps * std::max(Y, 1.0);
        hZ = eps * std::max(Z, 1.0);
        volatile double tempX = X + hX;
        volatile double tempY = Y + hY;
        volatile double tempZ = Z + hZ;
        hX = tempX - X;
        hY = tempY - Y;
        hZ = tempZ - Z;
    }
	
	template<typename Functor>
	inline V3d gradient(const V3d& P)
	{
		static const double eps = static_cast<double>(pow(static_cast<double>(DBL_EPSILON), 1.0/3.0));
		double hX, hY, hZ;
		getDeltas(hX, hY, hZ, P, eps);
		
		V3d dX(hX, 0.0, 0.0);
		V3d dY(0.0, hY, 0.0);
		V3d dZ(0.0, 0.0, hZ);
		double inv2hX = 0.5/hX;
		double inv2hY = 0.5/hY;
		double inv2hZ = 0.5/hZ;
		
		double Vxp = Functor::value(P + dX);
		double Vxm = Functor::value(P - dX);
		double Vyp = Functor::value(P + dY);
		double Vym = Functor::value(P - dY);
		double Vzp = Functor::value(P + dZ);
		double Vzm = Functor::value(P - dZ);
		double gradX = inv2hX * (Vxp - Vxm);
		double gradY = inv2hY * (Vyp - Vym);
		double gradZ = inv2hZ * (Vzp - Vzm);
		return V3d(gradX, gradY, gradZ);
	}
	
	template<typename Functor>
	inline M33d hessian(const V3d& P)
	{
		static const double eps = static_cast<double>(pow(static_cast<double>(DBL_EPSILON), 1.0/3.0));
		double hX, hY, hZ;
		getDeltas(hX, hY, hZ, P, eps);
		
		V3d dX(hX, 0.0, 0.0);
		V3d dY(0.0, hY, 0.0);
		V3d dZ(0.0, 0.0, hZ);
		
		V3d Gxp = gradient<Functor>(P + dX);
		V3d Gxm = gradient<Functor>(P - dX);
		V3d Gyp = gradient<Functor>(P + dY);
		V3d Gym = gradient<Functor>(P - dY);
		V3d Gzp = gradient<Functor>(P + dZ);
		V3d Gzm = gradient<Functor>(P - dZ);
		
		double inv2hX = 0.5/hX;
		double inv2hY = 0.5/hY;
		double inv2hZ = 0.5/hZ;
		double hXX = inv2hX*(Gxp[0] - Gxm[0]);
		double hYY = inv2hY*(Gyp[1] - Gym[1]); 
		double hZZ = inv2hZ*(Gzp[2] - Gzm[2]);
		double hXY = 0.5*(inv2hX*(Gxp[1] - Gxm[1]) + inv2hY*(Gyp[0] - Gym[0]));
		double hYZ = 0.5*(inv2hY*(Gyp[2] - Gym[2]) + inv2hZ*(Gzp[1] - Gzm[1]));
		double hXZ = 0.5*(inv2hZ*(Gzp[0] - Gzm[0]) + inv2hX*(Gxp[2] - Gxm[2]));
		
		V3d col0(hXX, hXY, hXZ);
		V3d col1(hXY, hYY, hXY);
		V3d col2(hXZ, hYZ, hZZ);
		M33d H;
		
		H[0][0] = hXX;
		H[1][0] = hXY;
		H[2][0] = hXZ;
		H[0][1] = hXY;
		H[1][1] = hYY;
		H[2][1] = hYZ;
		H[0][2] = hXZ;
		H[1][2] = hYZ;
		H[2][2] = hZZ;
		return H;
	}

	inline double trace(const M33d& F)
	{
		return F[0][0] + F[1][1] + F[2][2];
	}
	
	inline M33d adjoint(const M33d& F)
	{
		double Fxx = F[0][0];
		double Fyy = F[1][1];
		double Fzz = F[2][2];
		double Fxy = F[0][1];
		double Fxz = F[0][2];
		double Fyz = F[1][2];
		M33d A;
		A[0][0] = Fyy*Fzz - Fyz*Fyz;
		A[1][1] = Fxx*Fzz - Fxz*Fxz;
		A[2][2] = Fxx*Fyy - Fxy*Fxy;
		A[1][0] = Fyz*Fxz - Fxy*Fzz;
		A[2][0] = Fxy*Fyz - Fyy*Fxz;
		A[2][1] = Fxy*Fxz - Fxx*Fyz;
		A[0][1] = F[1][0];
		A[0][2] = F[2][0];
		A[1][2] = F[2][1];
		return A;
	}
	
	
	template<typename Functor>
	void calcCurvature(const V3d& P, double& k1, double& k2)
	{
		// Following "Curvature formulas for implicit curves and surfaces" by Ron Goldman
		V3d gradF = gradient<Functor>(P);
		M33d H = hessian<Functor>(P);
		M33d Ha = adjoint(H);
		double gradFSqr = gradF ^ gradF;
		double gaussian = (gradF^(gradF*Ha)) / (gradFSqr*gradFSqr + DBL_EPSILON);
		double mean = ((gradF^(gradF*H)) - gradFSqr*trace(H)) / (2.0*gradFSqr*gradF.length() + DBL_EPSILON);
		double det = sqrt(abs(mean*mean - gaussian));
		k1 = mean + det;
		k2 = mean - det;
	}
	
	
	template<typename Functor>
	float rocAtPoint(const V3d& P)
	{
		// Following "Curvature formulas for implicit curves and surfaces" by Ron Goldman
		double k1, k2; 
		calcCurvature<Functor>(P, k1, k2);
		
		double roc = 1.f / (max(abs(k1), abs(k2)) + FLT_EPSILON);
        return roc;
        //double maxRoc = 0.03f * min(min(m_extents[0], m_extents[1]), m_extents[2]);
		//return min(roc, maxRoc);
	}
    
    
	void computeOpeningAngle(list<Front::Vertex>::iterator vertex, Front* front)
	{
		list<Front::Vertex>::const_iterator prevVertex = vertex; 
		prevVertex--;
		if (prevVertex == front->m_vertices.end()) prevVertex--;
		
		list<Front::Vertex>::const_iterator nextVertex = vertex; 
		nextVertex++;
		if (nextVertex == front->m_vertices.end()) nextVertex++;
		
		const V3f& V = m_mesh.m_vertices[vertex->m_index];
		const V3f& N = m_mesh.m_vertexNormals[vertex->m_index];
		
		const V3f& Vprev = m_mesh.m_vertices[prevVertex->m_index];
		const V3f& Vnext = m_mesh.m_vertices[nextVertex->m_index];
		
		V3f X, Y;
		computeTangentSpace(N, X, Y);
		
		V3f Vprev_project(0);
		Vprev_project.x = (V - Vprev) ^ X;
		Vprev_project.y = (V - Vprev) ^ Y;
		Vprev_project.normalize();
		
		V3f Vnext_project(0);
		Vnext_project.x = (Vnext - V) ^ X;
		Vnext_project.y = (Vnext - V) ^ Y;
		Vnext_project.normalize();
		
		float dot = clamp(Vprev_project ^ Vnext_project, -1.f+FLT_EPSILON, 1.f-FLT_EPSILON);
		
		float openingAngle;
		if ((Vprev_project % Vnext_project).z >= 0.0f)
		{
            // reflex angle
			openingAngle = M_PI + acos(dot);
		}
		else
		{
            // acute angle
			openingAngle = M_PI - acos(dot); 
		}
		
		vertex->m_openingAngle = openingAngle;
	}
	
	
	template<typename Functor>
	V3f project(const V3f& start, float tol, int maxIterations=32)
	{
		float tolSqr = tol*tol;
		
		V3f x = start;
		float f = Functor::value(x);
		float fSqr = f*f;
		int nIter = 0;
		
		// Take Newton steps until the function magnitude falls below tolerance
		while (fSqr>tolSqr && nIter<maxIterations)
		{
			// (for the moment, ignore the issue of what to do if the gradient is close to zero).
			V3f grad = gradient<Functor>(x);
			//if ( (grad^grad) < tol ) { printf("project() failed, gradient magnitude too small"); exit(1); }
			
			// Take a Newton step 
			V3f step = f*grad/(grad^grad);
			
			// Reduce step size until the function magnitude falls, if needed 
			float fp, fpSqr;
			V3f xp;
			do 
			{
				nIter++;
				xp = x - step;
				fp = Functor::value(xp);
				fpSqr = fp*fp;
				step *= 0.5f;
			}
			while (fpSqr>=fSqr && nIter<maxIterations);
			
			x = xp;
			f = fp;
			fSqr = fpSqr;
		}
		
		if (nIter>=maxIterations) 
		{ 
			printf("warning, too many iterations in project"); 
            //exit(1); 
		}
		
		return x;
	}
    
    
    struct Prism
    {
        // Expansion prism centered at vertex V (index v), with corners A, B, height h.
        Prism(V3f V, int v, V3f A, V3f B, float h, bool tolA=false, bool tolB=false) : 
        
            m_V(V), m_v(v), m_A(A), m_B(B), m_h(h)
        
        { 
            V3f N = (B-V)%(A-V);
            N = N.normalize();
            // compute plane equations of prism faces, where plane normals point to the outside
            m_planeEquations[0] = _planeEquation( N, m_V+h*N);
            m_planeEquations[1] = _planeEquation(-N, m_V-h*N);
            m_planeEquations[2] = _planeEquation((N%(m_A-m_V)).normalize(), m_V);
            m_planeEquations[3] = _planeEquation((N%(m_V-m_B)).normalize(), m_B);
            m_planeEquations[4] = _planeEquation((N%(m_B-m_A)).normalize(), m_A);

            m_box.extendBy(m_V);
            m_box.extendBy(m_A);
            m_box.extendBy(m_B);
            
            m_tolDistA = tolA ? 1.0e-3f * (A-V).length() : -1.0f;
            m_tolDistB = tolB ? 1.0e-3f * (B-V).length() : -1.0f;
        }

        int m_v, m_a, m_b;
        float m_h;        
        V3f m_V, m_A, m_B;
        Box3f m_box;
        V4f m_planeEquations[5];
        float m_tolDistA, m_tolDistB;
                
        inline int getExpansionVertexIndex() const
        {
            return m_v;
        }
        
        inline V3f getVertexA() const
        {
            return m_A;
        }
        
        inline V3f getVertexB() const
        {
            return m_B;
        }
        
        inline Box3f getBoundingBox() const
        {
            return m_box;
        }
        
        inline V4f _planeEquation(const V3f& N, const V3f& P) const
        {
            return V4f(N[0], N[1], N[2], -P^N);
        }
        
        inline bool _clip(V3f& s0, V3f& s1, const V4f& plane) const
        {
            V3f n(plane[0], plane[1], plane[2]);
            float d = plane[3];
            float d0 = (s0^n) + d;
            float d1 = (s1^n) + d;
            if (d0>0.0  && d1>0.0) return false;   // Both points outside: trivial reject.                            
            if (d0<=0.0 && d1<=0.0) return true;   // Both points inside: trivial accept, no clip.
            float d0_abs = fabs(d0);
            float d1_abs = fabs(d1);
            float D = 1.f/(d0_abs+d1_abs+FLT_EPSILON);
            V3f X = D*(d0_abs*s1 + d1_abs*s0);     // Segment-plane intersection point computed as affine combination
            if (d1<0.0) s0 = X;                    // Determine which segment portion to clip away.
            else        s1 = X;
            return true;
        }
        
        inline V3f _closestPointOnLineSegment( const V3f& X, const V3f& a, const V3f& b ) const
        {
            V3f D = X - a;
            V3f BA = b - a;
            float t = clamp((D^BA)/((BA^BA) + FLT_EPSILON), 0.f, 1.f);
            return a + t*BA;
        }

        // Return whether segment (s0,  s1) intersects the prism, and if so, the point on the segment closest to V.
        bool segmentCollide(const V3f& s0, const V3f& s1, V3f& C) const
        {
            // clip segment to the interior side of the 5 faces
            V3f x0(s0), x1(s1);
            
            if ( _clip(x0, x1, m_planeEquations[0]) &&
                 _clip(x0, x1, m_planeEquations[1]) &&
                 _clip(x0, x1, m_planeEquations[2]) &&
                 _clip(x0, x1, m_planeEquations[3]) &&
                 _clip(x0, x1, m_planeEquations[4]) )
            {
                // We have a segment [x0, x1] which lies entirely within the prism.
                // Determine the closest point on this segment to V.
                C = _closestPointOnLineSegment(x0, x1, m_V);
                
                // Check to prevent spurious intersections at edges which join to the prism
                if ((C-m_A).length()<m_tolDistA) return false;
                if ((C-m_B).length()<m_tolDistB) return false;
                
                return true;
            }
            else 
            {
                return false;
            }            
        }
        
    };
    
    
    struct Bridge
    {
        Edge m_edge; // mesh edge on which closest hit occurs
        V3f m_C;     // the closest feature point on this edge
        int m_front; // index of the front containing this edge
    };
    
    
    bool prismsCheck(const vector<Prism>& prisms, const Geometry& mesh, Bridge& closestBridge)
    {
        float minDist = FLT_MAX;
        bool hit=false;
        
        for (int n=0; n<prisms.size(); ++n)
        {
            const Prism& prism = prisms[n];
            int vertex = prism.getExpansionVertexIndex();
            V3f V = m_mesh.m_vertices[vertex];
            
            Bridge bridge;
            if (prismCheck(prism, mesh, bridge))
            {
                hit = true;
                float dist = (V - bridge.m_C).length();
                if (dist<minDist)
                {
                    minDist = dist;
                    closestBridge = bridge;
                }
            }
        }
        
        return hit;
    }
    
    
    /*  For now - just a global check of the given prism against all fronts */
    bool prismCheck(const Prism& prism, const Geometry& mesh, Bridge& closestBridge)
    {
        /*
         - have to test the prism against *all* other fronts
         - find all edges (of any front) which collide with the prism.
         - for each hit, determine the closest point on the clipped edge, C.
         - if there are no hits, we can proceed with the expansion.
         */
        
        int vertex = prism.getExpansionVertexIndex();
        V3f V = m_mesh.m_vertices[vertex];
        V3f Prev = prism.getVertexA();
        V3f Next = prism.getVertexB();
        
        vector<Bridge> candidateBridges;
        
        for (int f=0; f<m_fronts.size(); ++f)
		{
			const Front* front = m_fronts[f];
            
            vector<int> leafIndices;
            front->m_edgeBVH.query(prism.getBoundingBox(), leafIndices);

            for (int i=0; i<leafIndices.size(); ++i)
            {
                int leafIndex = leafIndices[i];
                Edge edge = front->m_edgeBVH.getLeafData(leafIndex);
                
                // Ignore edges containing the prism expansion vertex, since these
                // cannot genuinely intersect the prism (by construction).
                if (edge.m_a==vertex || edge.m_b==vertex) continue;
        
                V3f C;
                
                if ( prism.segmentCollide(edge.m_A, edge.m_B, C) )
                {
                    Bridge bridge;
                    bridge.m_edge = edge;
                    bridge.m_C = C;
                    bridge.m_front = f;
                    candidateBridges.push_back(bridge);
                }
            }
        }
        
        if (candidateBridges.size()==0) return false;
        
        closestBridge = candidateBridges[0];
        float minDistSqr = (closestBridge.m_C - V).length2();
        for (int b=1; b<candidateBridges.size(); ++b)
        {
            const Bridge& bridge = candidateBridges[b];
            float distSqr = (bridge.m_C - V).length2();
            if (distSqr < minDistSqr)
            {
                closestBridge = bridge;
                minDistSqr = distSqr;
            }
        }
        
        return true;
    }
    

    
    inline Geometry::Triangle _makeTriangle(int a, int b, int c)
    {
        Geometry::Triangle tri;
        tri.m_vertex[0] = a;
        tri.m_vertex[1] = b;
        tri.m_vertex[2] = c;
        return tri;
    }
    
    
    int _otherTriVertex(int i, int j, const Geometry::Triangle& tri) const
    {
        int a = tri.m_vertex[0];
        int b = tri.m_vertex[1];
        int c = tri.m_vertex[2];
        if ((a!=i) and (a!=j)) return a;
        if ((b!=i) and (b!=j)) return b;
        return c;
    }
    
    // 'Fill' a front with <=4 vertices (Does not do anything to a front with greater than 4 vertices)
    void fillHole(Front* F)
	{
		vector<int> indices;
		list<Front::Vertex>::const_iterator vertex = F->m_vertices.begin();
		while (vertex != F->m_vertices.end())
		{	
			indices.push_back(vertex->m_index);
			vertex++;
		}
		
		if (indices.size()>4) return;
        assert(indices.size()>=3);
        {
            Geometry::Triangle tri;
            tri.m_vertex[0] = indices[0];
            tri.m_vertex[1] = indices[1];
            tri.m_vertex[2] = indices[2];
            m_mesh.m_triangles.push_back(tri);
        }
		
        if (indices.size()>3)
		{
            Geometry::Triangle tri;
            tri.m_vertex[0] = indices[2];
            tri.m_vertex[1] = indices[3];
            tri.m_vertex[2] = indices[0];
            m_mesh.m_triangles.push_back(tri);
		}	
        
        // Rebuild the front BVH
        F->rebuildEdgeBVH(m_mesh);
	}
    
    
    // Given two fronts A and B, merge them into a single new front (which is created and returned, without
    // modifying the input fronts), by bridging between an edge of A (a->a'), and an edge of B (b->b'),
    // forming links b->a', a->b'.
    Front* mergeFronts(Front* A, Front* B, int a, int b)
    {
        // Build front F: a -> b' -> ... -> b -> a' -> ... -> wraparound to a
        int ap = A->nextVertex(a);
        int bp = B->nextVertex(b);
        
        Front* F = new Front();
        F->m_vertices.push_back(Front::Vertex(a));
        
        // add link a->b'
        F->insertVertex(a, bp);
        
        // add links from B, b'-> ... ->b
        VertexIter iterB = B->nextIter(B->getIterator(bp));
        while (iterB->m_index != b)
        {
            F->insertVertex(NULL, iterB->m_index);
            if (++iterB == B->m_vertices.end()) iterB++;
        }
        
        // add link b->a'
        F->insertVertex(b, ap);
        
        // add links from A, a'-> ... ->a
        VertexIter iterA = A->nextIter(A->getIterator(ap));
        while (iterA->m_index != ap)
        {
            F->insertVertex(NULL, iterA->m_index);
            if (++iterA == A->m_vertices.end()) iterA++;
        }
    
        return F;
    }
    
    
    // Given a front with edges (a,a'), (b,b'), split it into two new fronts, A and B,
    // 'cutting' by forming links b->a', a->b'.
    void splitFront(Front* F, int a, int b, Front* &A, Front* &B)
    {
        int ap = A->nextVertex(a);
        int bp = B->nextVertex(b);
        
        // Build front A: a -> b' -> wraparound to a
        A = new Front();
        {
            A->m_vertices.push_back(Front::Vertex(a));
            
            // add link a->b'
            A->insertVertex(a, bp);
            
            // add links b'-> ... ->a
            VertexIter iter = F->nextIter(F->getIterator(bp));
            while (iter->m_index != a)
            {
                A->insertVertex(NULL, iter->m_index);
                if (++iter == F->m_vertices.end()) iter++;
            }
        }
        
        // Build front B: b -> a' -> wraparound to b
        B = new Front();
        {
            B->m_vertices.push_back(Front::Vertex(b));

            // add link b->a'
            B->insertVertex(b, ap);
            
            // add links a'-> ... ->b
            VertexIter iter = F->nextIter(F->getIterator(ap));
            while (iter->m_index != b)
            {
                B->insertVertex(NULL, iter->m_index);
                if (++iter == F->m_vertices.end()) iter++;
            }
        }
    }
    
    
    void bridge(int currentFront, const Bridge& bridge, list<Front::Vertex>::iterator& Viter)
    {
        const Edge& hitEdge = bridge.m_edge;
        int hitFront = bridge.m_front;
        
        Front* front = m_fronts[currentFront];
		assert(front->m_vertices.size()>4);
        
        list<Front::Vertex>::iterator Piter = Viter; Piter--;
		if (Piter == front->m_vertices.end()) Piter--;
		
		list<Front::Vertex>::iterator Niter = Viter; Niter++;
		if (Niter == front->m_vertices.end()) Niter++;
        
        const V3f& V = m_mesh.m_vertices[Viter->m_index];
        const V3f& P = m_mesh.m_vertices[Piter->m_index];

        // Get the vertices of the hit edge on the colliding front.
		Front* otherFront = m_fronts[hitFront];
        list<Front::Vertex>::iterator Aiter = otherFront->m_vertexIndexToFrontIterator[hitEdge.m_a];
		list<Front::Vertex>::iterator Biter = otherFront->m_vertexIndexToFrontIterator[hitEdge.m_b];
        const V3f& A = m_mesh.m_vertices[Aiter->m_index];
		const V3f& B = m_mesh.m_vertices[Biter->m_index];
        // We know that A->B is the correct order of the vertices in the hit front linked list.

        // The point to connect to on the hit edge
        V3f C = bridge.m_C;
        
        // If C is within a small tolerance of the hit edge vertices, snap it to those vertices.
        float ab = (hitEdge.m_A - hitEdge.m_B).length();
        bool connectToA = (C - hitEdge.m_A).length() < 5.e-2f * ab;
        bool connectToB = (C - hitEdge.m_B).length() < 5.e-2f * ab;
        
        // Need to find triangles corresponding to hit edge A->B, and P->V edge.
        int bustedTriangleABZ = Aiter->m_triangleIndex;
        int bustedTrianglePVW = Piter->m_triangleIndex;
        const Geometry::Triangle& tri_ABZ = m_mesh.m_triangles[bustedTriangleABZ];
        const Geometry::Triangle& tri_PVW = m_mesh.m_triangles[bustedTrianglePVW];
        
        // Find vertices Z and W, from ABZ and PVW
        int Z_index = _otherTriVertex(Aiter->m_index, Biter->m_index, tri_ABZ);
        int W_index = _otherTriVertex(Piter->m_index, Viter->m_index, tri_PVW);
    
        // epsilon bridge
        float e = 0.05f;
        
        // construct V' = V*(1-e) + P*e (and insert V' into the current front)
        V3f Vp = V*(1.f-e) + P*e;
        
        m_mesh.m_vertices.push_back(Vp);
        int Vp_index = m_mesh.m_vertices.size()-1;
        front->insertVertex(Piter->m_index, Vp_index);
        
        int C_index = -1;
        int Cp_index = -1;
        
        int bridge_span_start_vertex;
        
        //////////////////////////////////////////////////////////////////////////////////////////
        // Now:
        //    - create new geometry, depending on the snapping choice (remove triangles ABZ, PVW,
        //      add new ones to introduce bridge connection points C, and optionally Cp).
        //    - modify the Front(s), to insert the newly added vertices.
        //////////////////////////////////////////////////////////////////////////////////////////
        if (connectToA)
        {
            ////////////////////////////////////////////////////////////
            // case Ia
            ////////////////////////////////////////////////////////////
            
            // construct C' = a*(1-e) + b*e (and insert C' into the hit front)
            V3f Cp = A*(1.f-e) + B*e;
            
            m_mesh.m_vertices.push_back(Cp);
            Cp_index = m_mesh.m_vertices.size()-1;
            otherFront->insertVertex(Aiter->m_index, Cp_index);
            
            // Replace triangle ABZ with C'BZ
            Geometry::Triangle tri_CpBZ  = _makeTriangle(Cp_index,       Biter->m_index, Z_index);
            m_mesh.m_triangles[bustedTriangleABZ] = tri_CpBZ;
            
            // Replace triangle PVW with PV'W
            Geometry::Triangle tri_PVpW  = _makeTriangle(Piter->m_index, Vp_index,       W_index);
            m_mesh.m_triangles[bustedTrianglePVW] = tri_PVpW;
            
            // Add triangles AC'Z, V'VW, VV'A, V'C'A
            Geometry::Triangle tri_ACpZ  = _makeTriangle(Aiter->m_index, Cp_index,       Z_index);
            Geometry::Triangle tri_VpVW  = _makeTriangle(Vp_index,       Viter->m_index, W_index);
            Geometry::Triangle tri_VVpA  = _makeTriangle(Viter->m_index, Vp_index,       Aiter->m_index);
            Geometry::Triangle tri_VpCpA = _makeTriangle(Vp_index,       Cp_index,       Aiter->m_index);
            
            m_mesh.m_triangles.push_back(tri_ACpZ);
            m_mesh.m_triangles.push_back(tri_VpVW);
            m_mesh.m_triangles.push_back(tri_VVpA);
            m_mesh.m_triangles.push_back(tri_VpCpA);
            
            // Store the vertex index where the edge from V joins the fronts (one edge of the bridge)
            bridge_span_start_vertex = Aiter->m_index;
            
            // [Flag edge AC' for edge collapse]

        }
    
        else if (connectToB)
        {
            ////////////////////////////////////////////////////////////
            // case Ib
            ////////////////////////////////////////////////////////////
            
            // construct C' = b*(1-e) + a*e (and insert C' into the hit front)
            V3f Cp = B*(1.f-e) + A*e;

            m_mesh.m_vertices.push_back(Cp);
            Cp_index = m_mesh.m_vertices.size()-1;
            otherFront->insertVertex(Aiter->m_index, Cp_index);
            
            // Replace triangle ABZ with AC'Z
            Geometry::Triangle tri_ACpZ  = _makeTriangle(Aiter->m_index, Cp_index,       Z_index);
            m_mesh.m_triangles[bustedTriangleABZ] = tri_ACpZ;
                        
            // Replace triangle PVW with PV'W
            Geometry::Triangle tri_PVpW  = _makeTriangle(Piter->m_index, Vp_index,       W_index);
            m_mesh.m_triangles[bustedTrianglePVW] = tri_PVpW;
            
            // Add triangles: C'BZ, V'VW, VV'B, VBC'
            Geometry::Triangle tri_CpBZ  = _makeTriangle(Cp_index,       Biter->m_index, Z_index);
            Geometry::Triangle tri_VpVW  = _makeTriangle(Vp_index,       Viter->m_index, W_index);
            Geometry::Triangle tri_VVpB  = _makeTriangle(Viter->m_index, Vp_index,       Biter->m_index);
            Geometry::Triangle tri_VBCp  = _makeTriangle(Viter->m_index, Biter->m_index, Cp_index);
            
            m_mesh.m_triangles.push_back(tri_CpBZ);
            m_mesh.m_triangles.push_back(tri_VpVW);
            m_mesh.m_triangles.push_back(tri_VVpB);
            m_mesh.m_triangles.push_back(tri_VBCp);
            
            // Store the vertex index where the edge from V joins the fronts (one edge of the bridge)
            bridge_span_start_vertex = Cp_index;
            
            // [Flag edge BC' for edge collapse]
            
        }
        
        else
        {
            ////////////////////////////////////////////////////////////
            // case II
            ////////////////////////////////////////////////////////////
            
            // Insert C into the hit front:            
            m_mesh.m_vertices.push_back(C);
            C_index = m_mesh.m_vertices.size()-1;
            otherFront->insertVertex(Aiter->m_index, C_index);
            
            // construct: C' = C*(1-e) + b*e (and insert C' into the hit front)
            V3f Cp = C*(1.f-e) + B*e;
            
            m_mesh.m_vertices.push_back(Cp);
            Cp_index = m_mesh.m_vertices.size()-1;
            otherFront->insertVertex(C_index, Cp_index);
            
            // Replace triangle ABZ with C'BZ
            Geometry::Triangle tri_CpBZ  = _makeTriangle(Cp_index,        Biter->m_index, Z_index);
            m_mesh.m_triangles[bustedTriangleABZ] = tri_CpBZ;
            
            // Replace triangle PVW with PV'W
            Geometry::Triangle tri_PVpW  = _makeTriangle(Piter->m_index,  Vp_index,       W_index);
            m_mesh.m_triangles[bustedTrianglePVW] = tri_PVpW;
            
            // Add triangles: ACZ, CC'Z, V'VW, V'C'C, VV'C
            Geometry::Triangle tri_ACZ   = _makeTriangle(Aiter->m_index,  C_index,        Z_index);
            Geometry::Triangle tri_CCpZ  = _makeTriangle(C_index,         Cp_index,       Z_index);
            Geometry::Triangle tri_VpVW  = _makeTriangle(Vp_index,        Viter->m_index, W_index);
            Geometry::Triangle tri_VpCpC  = _makeTriangle(Vp_index,       Cp_index,       C_index);
            Geometry::Triangle tri_VVpC   = _makeTriangle(Viter->m_index, Vp_index,       C_index);
            
            m_mesh.m_triangles.push_back(tri_ACZ);
            m_mesh.m_triangles.push_back(tri_CCpZ);
            m_mesh.m_triangles.push_back(tri_VpVW);
            m_mesh.m_triangles.push_back(tri_VpCpC);
            m_mesh.m_triangles.push_back(tri_VVpC);
            
            // Store the vertex index where the edge from V joins the fronts (one edge of the bridge)
            bridge_span_start_vertex = C_index;
            
            // [Flag edge CC' for edge collapse]
            
        }
            
        // Flag edge V'V for edge collapse
        // [edge collapse at the soonest opportunity (as soon as vertices on collapsed edges are no longer on any front)]
    
        /////////////////////////////////////////////////////////////////////////////////////
        // Now generate new Fronts with the appropriate topology.
        /////////////////////////////////////////////////////////////////////////////////////
        
        if (hitFront == currentFront)
        {
            printf("-------------- hitFront == currentFront = %d ---------------", currentFront);
            printf("\nSPLIT\n");
            
            // Split existing front F into two. Say the old vertex order in F was: 
            // F: a -> [c] -> [c'] -> b -> ... -> p -> v' -> v -> n -> ... a_bef -> wraparound to a
            
            // We split F to create two new fronts F0, F1:
            // F0: ...  a -> [c] -> v -> n -> ... -> a_bef -> wraparound to a
            // F1: ...  v' -> [c'] -> b -> b_aft -> ... -> p -> wraparound to v'
            
            Front* A;
            Front* B;
            splitFront(front, bridge_span_start_vertex, Vp_index, A, B);
                    
            delete m_fronts[currentFront];
			m_fronts.erase(m_fronts.begin() + currentFront);
            
            if (A->m_vertices.size()>4)
			{
                A->rebuildEdgeBVH(m_mesh);
                m_fronts.push_back(A);
            }
            else
            {
                fillHole(A);
                delete A;
            }
            
            if (B->m_vertices.size()>4)
			{
                B->rebuildEdgeBVH(m_mesh);
                m_fronts.push_back(B);
            }
            else
            {
                fillHole(B);
                delete B;
            }
        }
        else 
        {
            printf("-------------- hitFront = %d != currentFront = %d ---------------", hitFront, currentFront);
			printf("\nMERGE\n");
			
			// Say the old front vertex orders were:
			// FA: a -> a_aft -> ... -> a_bef -> wraparound to a
			// FB: b -> b_aft -> ... -> b_bef -> wraparound to a
			
            // We merge FA and FB to create one new front F:
			// F: a -> b_aft -> ... -> b_bef -> b -> a_aft -> ... -> a_bef -> wraparound to a
            
            Front* F = mergeFronts(front, otherFront, Vp_index, bridge_span_start_vertex);
            
            delete m_fronts[currentFront];
			delete m_fronts[hitFront];
            
			vector<Front*> newFronts;
			for (int f=0; f<m_fronts.size(); ++f)
			{
				if (f!=currentFront && f!=hitFront)
				{
					newFronts.push_back(m_fronts[f]);
				}
			}
			m_fronts = newFronts;
            
            if (F->m_vertices.size()>4)
			{
                F->rebuildEdgeBVH(m_mesh);
                m_fronts.push_back(F);
            }
            else 
            {
                fillHole(F);
                delete F;
            }
        }
    
    }
    
    
    inline V3f rotateZ(const V3f& vIn, float angleRadians)
	{
		float s = sin(angleRadians);
		float c = cos(angleRadians);
		V3f vOut;
		vOut.x = c*vIn.x - s*vIn.y;
		vOut.y = s*vIn.x + c*vIn.y;
		vOut.z = 0.0f;
		return vOut;
	}
    
    
    void switchFront()
    {
        int numFronts = m_fronts.size();
        m_currentFront++;
        if (m_currentFront>=numFronts) m_currentFront=0;
        m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
        m_switchFront = false;
    }
    

	template<typename Functor>
	bool grow()
	{			
        if (m_switchFront) switchFront();
		if (m_fronts.size()==0) return false;
        
		Front* front = m_fronts[m_currentFront];
        
		if (front->m_vertices.size()<=4)
		{
			fillHole(front);
			delete m_fronts[m_currentFront];
			m_fronts.erase(m_fronts.begin() + m_currentFront);
			
			m_switchFront = true;
			if (m_switchFront)
			{
				int numFronts = m_fronts.size();
				m_currentFront++;
				if (m_currentFront>=numFronts) m_currentFront=0;
				m_currentFrontVertex = m_fronts[m_currentFront]->m_vertices.begin();
				m_switchFront = false;
			}
			
			return false;
		}
		
		list<Front::Vertex>::iterator frontVertex = m_currentFrontVertex;
		if (frontVertex == front->m_vertices.end())
		{
            switchFront();
			return false;
		}
		
		printf("----------------  grow  -----------------\n\n");
		
        //debug_printFronts();
		
		// Choose best front vertex
		//list<Front::Vertex>::iterator minAngleVertex = chooseFrontVertex(front);
        //frontVertex = minAngleVertex;
		
		list<Front::Vertex>::iterator prevVertex = frontVertex; prevVertex--;
		if (prevVertex == front->m_vertices.end()) prevVertex--;
		
		list<Front::Vertex>::iterator nextVertex = frontVertex; nextVertex++;
		if (nextVertex == front->m_vertices.end()) nextVertex++;
		
		int debug_frontVertex = frontVertex->m_index;
		printf("%d\n", debug_frontVertex);
		
		int debug_prevVertex = prevVertex->m_index;
		printf("%d\n", debug_prevVertex);
		
		int debug_nextVertex = nextVertex->m_index;
		printf("%d\n", debug_nextVertex);
		
		int numVertices = m_mesh.m_vertices.size();
		int numNormals = m_mesh.m_vertexNormals.size();
		
		const V3f& V = m_mesh.m_vertices[frontVertex->m_index];
		const V3f& N = m_mesh.m_vertexNormals[frontVertex->m_index];
		const V3f& Vprev = m_mesh.m_vertices[prevVertex->m_index];
		const V3f& Vnext = m_mesh.m_vertices[nextVertex->m_index];
		
		V3f X, Y;
		computeTangentSpace(N, X, Y);
		
		V3f Vprev_project(0);
		Vprev_project.x = (V - Vprev) ^ X;
		Vprev_project.y = (V - Vprev) ^ Y;
		Vprev_project.normalize();
		
		V3f Vnext_project(0);
		Vnext_project.x = (Vnext - V) ^ X;
		Vnext_project.y = (Vnext - V) ^ Y;
		Vnext_project.normalize();
		
		float dot = clamp(Vprev_project ^ Vnext_project, -1.f+FLT_EPSILON, 1.f-FLT_EPSILON);
		
		float openingAngle;
		if ((Vprev_project % Vnext_project).z >= 0.0f)
		{
            // reflex angle
			openingAngle = M_PI + acos(dot);
		}
		else
		{
            // acute angle
			openingAngle = M_PI - acos(dot); 
		}
		
		frontVertex->m_openingAngle = openingAngle;
		
		int numPoints = floor(3.0*openingAngle/M_PI);
		

        // Construct the expansion prisms
        float roc = m_vertexRocs[frontVertex->m_index];
        float height = 2.f * m_scale * m_scale * roc;
        
        ////////////////////////////////////////////////////////////////////
        // Ear cut
        ////////////////////////////////////////////////////////////////////
        if (numPoints==0)
        {
            // special case, prism formed by 'cutting the ear'
            Prism prism(V, frontVertex->m_index, Vprev, Vnext, height, true, true);
            
            Bridge closestBridge;
            bool hit = prismCheck(prism, m_mesh, closestBridge);
            
            /////////////// Expand ///////////////
            if (!hit)
            {
                // - if there are no other genuine hits, we can proceed with the expansion.
             
                // Modify the front vertex list
				front->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
				front->m_vertices.erase(frontVertex);

                // Add new triangle to mesh
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = prevVertex->m_index;
					tri.m_vertex[2] = nextVertex->m_index;
					m_mesh.m_triangles.push_back(tri);
				}
                
                front->rebuildEdgeBVH(m_mesh);
                
                // update the front triangle index of the previous vertex
                prevVertex->m_triangleIndex = m_mesh.m_triangles.size()-1;
                
                prevVertex->m_active = true;
				nextVertex->m_active = true;
                m_currentFrontVertex = nextVertex;
                return true;
            }
            
            /////////////// Modify topology ///////////////
            else 
            {
                bridge(m_currentFront, closestBridge, frontVertex);
                switchFront();
            }
        }
        
        ////////////////////////////////////////////////////////////////////
        // Multiple prisms
        ////////////////////////////////////////////////////////////////////
        else
        {
            // Make multiple prisms. If we get a hit for any of these prisms, we make an epsilon bridge to the hit.
            // TO DO: possibly, allow an ear-cut in cases where the angle is not too large, and the ear-prism fits.
            
            vector<V3f> newPoints;
            float angle = openingAngle / float(numPoints+1);
            
            for (int n=0; n<numPoints; ++n)
			{
				V3f newPoint_project = rotateZ(-Vprev_project, float(n+1)*angle);
				V3f newPoint_dir = (newPoint_project.x*X + newPoint_project.y*Y).normalize();
				V3f newPoint = V + (2.f*roc*newPoint_dir);
				V3f projectedPoint = project<Functor>(newPoint, m_tol);
				newPoints.push_back(projectedPoint);
			}
            
            vector<Prism> expansionPrisms;
            {
                Prism prism(V, frontVertex->m_index, Vprev, newPoints[0], height, true, false);
                expansionPrisms.push_back(prism);
            }
            for (int n=1; n<numPoints; ++n)
			{
                Prism prism(V, frontVertex->m_index, newPoints[n-1], newPoints[n], height, false, false);
                expansionPrisms.push_back(prism);
            }
            {
                Prism prism(V, frontVertex->m_index, newPoints[numPoints-1], Vnext, height, false, true);
                expansionPrisms.push_back(prism);
            }
                       
            
            Bridge closestBridge;
            bool hit = prismsCheck(expansionPrisms, m_mesh, closestBridge);
            
            /////////////// Expand ///////////////
            if (!hit)
            {
                // Add the new vertices and triangles to the mesh, and modify the front accordingly.
				int vertexNum = m_mesh.m_vertices.size();
                                
                // Add new vertices to mesh
				for (int n=0; n<numPoints; ++n)
				{
					m_mesh.m_vertices.push_back(newPoints[n]);
					m_vertexRocs.push_back( rocAtPoint<Functor>(newPoints[n]) );
				}
             
                int triangleIndexBase = m_mesh.m_triangles.size();
                
                // Add new triangles to mesh
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = prevVertex->m_index;
					tri.m_vertex[2] = vertexNum+0;
					m_mesh.m_triangles.push_back(tri);
				}
                
                for (int n=0; n<numPoints-1; ++n)
				{
					Geometry::Triangle tri;                                                                                                                       
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = vertexNum+n;
					tri.m_vertex[2] = vertexNum+n+1;
					m_mesh.m_triangles.push_back(tri);
				}
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = frontVertex->m_index;
					tri.m_vertex[1] = vertexNum+numPoints-1;
					tri.m_vertex[2] = nextVertex->m_index;
					m_mesh.m_triangles.push_back(tri);
				}
                
                // Compute the new vertex normals
				for (int n=0; n<numPoints; ++n)
				{
					V3f N = gradient<Functor>(newPoints[n]);
					N.normalize();
					int debug_normal_index = m_mesh.m_vertexNormals.size();
					m_mesh.m_vertexNormals.push_back(N);
				}

                // Modify the front vertex list to insert the fan
				list<Front::Vertex>::iterator iter = nextVertex; 
				for (int n=numPoints-1; n>=0; n--)
				{
					Front::Vertex newVertex(vertexNum+n);
					iter = front->m_vertices.insert(iter, newVertex);
					front->m_vertexIndexToFrontIterator[vertexNum+n] = iter;
                    //computeOpeningAngle(iter, front);
                    
                    // We add 1 more triangle than the number of added points..
                    newVertex.m_triangleIndex = triangleIndexBase + n + 1;
				}
				front->m_vertexIndexToFrontIterator.erase(frontVertex->m_index);
				front->m_vertices.erase(frontVertex);

                // update the front triangle index of the previous vertex (that of the next vertex is unchanged)
                prevVertex->m_triangleIndex = triangleIndexBase;
                                
                prevVertex->m_active = true;
				nextVertex->m_active = true;
                
                front->rebuildEdgeBVH(m_mesh);
                
                m_currentFrontVertex = nextVertex;
            }
            
            /////////////// Modify topology ///////////////
            else 
            {
                bridge(m_currentFront, closestBridge, frontVertex);
                switchFront();
            }
            
        }
    
    }
    
    
    template<typename Functor>
    void Polygonizer::seedHexagon(V3f startPoint)
    {
        // Project it to the isosurface
        V3f seedPoint = project<Functor>(startPoint, m_tol);
        
        V3f grad = gradient<Functor>(seedPoint);
        
        V3f n = grad;
        n.normalize();
        
        V3f t, b;
        computeTangentSpace(n, t, b);
        
        float roc = rocAtPoint<Functor>(seedPoint);
        
        m_mesh.m_vertices.push_back(seedPoint);
        m_mesh.m_vertexNormals.push_back(n);
        m_vertexRocs.push_back(roc);
        
        // Generate initial front, which is directed anticlockwise from above
        // (i.e., the outside of the front is to the right as you walk along the directed front with your head in the normal/out direction).
        for (int i=0; i<6; ++i)
        {
            float x = 2.f * roc * cos(float(i) * 0.3333333f*M_PI);
            float y = 2.f * roc * sin(float(i) * 0.3333333f*M_PI);
            
            V3f projectedPoint = project<Functor>(seedPoint + x*t + y*b, m_tol);
            m_mesh.m_vertices.push_back(projectedPoint);
            
            V3f N = gradient<Functor>(projectedPoint);
            N.normalize();
            
            m_mesh.m_vertexNormals.push_back(N);
            
            m_vertexRocs.push_back( rocAtPoint<Functor>(projectedPoint) );
        }
        
        Front* front = new Front();
        for (int i=0; i<6; ++i)
        {
            int vertexIndex = 1 + (i%6);
            front->m_vertices.push_back(Front::Vertex(vertexIndex));
            
            list<Front::Vertex>::iterator iter = front->m_vertices.end();
            iter--;
            
            front->m_vertexIndexToFrontIterator[vertexIndex] = iter;
            //computeOpeningAngle(iter, front);
        }
        m_fronts.push_back(front);
        
        for (int i=0; i<6; ++i)
        {
            Geometry::Triangle tri;                                                                                                                       
            tri.m_vertex[0] = 0;
            tri.m_vertex[1] = 1+(i%6);
            tri.m_vertex[2] = 1+((i+1)%6);
            m_mesh.m_triangles.push_back(tri);
            
            // Associate the front vertices with their adjacent triangle
            list<Front::Vertex>::iterator V = front->m_vertexIndexToFrontIterator[1+(i%6)];
            V->m_triangleIndex = m_mesh.m_triangles.size()-1;
        }	
        
        front->rebuildEdgeBVH(m_mesh);
    }


};





    




#endif //POLYGONIZER3_H





